/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package dynamixel;

import MouseSensorDriver.Calibrator;
import java.io.Serializable;
import java.util.logging.Logger;

/**
 *
 * @author Asko
 */
public class EndoscopeGear implements Serializable{
       
    Actuator actuator;
    String unitPosition;
    String unitTorque;
    double maxPos, minPos;
    
    public EndoscopeGear(Actuator actuator, String unitPosition,
            String unitTorque, double maxPos, double minPos) {
        this.actuator = actuator;
        this.unitPosition = unitPosition;
        this.unitTorque = unitTorque;
        this.maxPos = maxPos;
        this.minPos = minPos;
    }
    
    public void setTorque(double torque) {
        actuator.setTorqueLimit(Calibrator.get().getSourceData(actuator.
                getEngineId() * 2 - 1 + Calibrator.ES_MOTOR_POS_1, torque));
    }

    public double getTorque() {
        return Calibrator.get().getData(actuator.
                getEngineId() * 2 - 1 + Calibrator.ES_MOTOR_POS_1,
                actuator.getCurTorque() / 2);
    }
     
    public double getTorque(int val) {
        return Calibrator.get().getData(actuator.
                getEngineId() * 2 - 1 + Calibrator.ES_MOTOR_POS_1,
                val / 2);
    }
    
    public double getCupboardTorqueEng(double val) {
        return Calibrator.get().getData(actuator.getEngineId()+ 
                Calibrator.ENDO_SENSOR_1_ENG-1,(long)val);
    }
    
    public double getCupboardTorqueADC(double val) {
        return Calibrator.get().getData(actuator.getEngineId()+ 
                Calibrator.ENDO_SENSOR_1_ADC-1,(long)val);
    }

    public double getPosition() {
        int val = actuator.getCurPos();
        if(val != -1 && val>=0 && val<=4095){            
        //System.out.println("val: "+val);
        return Calibrator.get().getData(actuator.
                getEngineId() * 2 - 2
                + Calibrator.ES_MOTOR_POS_1, val);
        }else{
            return Double.MAX_VALUE;
        }
    }

    public double getPosition(int val) {
        return Calibrator.get().getData(actuator.
                getEngineId() * 2 - 2
                + Calibrator.ES_MOTOR_POS_1, val);
    }

    public void setPosition(double pos) {
       // System.out.println("Starting setting pos.");
        if (pos <= maxPos && pos >= minPos) {
            double posCalib = Calibrator.get().getSourceData(actuator.
                    getEngineId() * 2 - 2 + Calibrator.ES_MOTOR_POS_1, pos);
            actuator.setGoalPosition((int) posCalib);
      //      System.out.println("Pos setted: " + posCalib);
        } else {
            System.out.println("Shit happened");
        }
    }
    
    public double getMaxPos() {
        return maxPos;
    }

    public double getMinPos() {
        return minPos;
    }

    public String getUnitPosition() {
        return unitPosition;
    }

    public String getUnitTorque() {
        return unitTorque;
    }

    public Actuator getActuator() {
        return actuator;
    }
}
